/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file template.cxx
	\brief class implemenation for template

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "nidaq.h"

  /*<-------Defines--------->*/
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
  /*<-End Global Variables-->*/

NIDAQ::NIDAQ(String name, int port) : ThreadedPipe(name)
{
  num_connections = 0;
  this->port = port;
}


bool NIDAQ::init(ShmStruct *shmStructPtr) {
  ThreadedPipe::init(shmStructPtr);
  int r;

  servSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
  if (servSocket < 0) {
    printf("socket failed; %s\n", strerror(errno));
  } else {
    server_info.sin_family = AF_INET;
    server_info.sin_addr.s_addr = inet_addr("127.0.0.1");
    server_info.sin_port = htons(port);
    for (int i=0; i<4; i++)
      printf("%d.", ((uint8_t*)&(server_info.sin_addr.s_addr))[i]);
    printf("\n");

    r = bind(servSocket, (struct sockaddr*)&server_info, sizeof(server_info));
    if (r < 0) {
      printf("bind failed %s\n", strerror(errno));
    } else {
      r = listen(servSocket, MAX_CONNECTIONS);
      if (r < 0) {
        printf("listen failed %s\n", strerror(errno));
      }
      // fork so we're blocked on the socket!
      g_thread_init(NULL);
      g_thread_create(acceptConnections, this, FALSE, NULL);
    }
  }
  printf("successfully setup server!\n");

  return true;
}

void NIDAQ::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  // No outboard because we're manually reading/writing from the DAQ

  fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  for (int i=0; i<num_connections; i++) {
    FD_SET(client_fds[i], &readFDs);
    if (client_fds[i] > maxFD) maxFD = client_fds[i];
  }

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("NIDAQ::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {

    // maintain other connections
    for (int i = 0; i < num_connections; i++) {
      if (FD_ISSET(client_fds[i], &readFDs)) {
        printf("recieved something from a tcp client (shouldn't have!)\n");
        // read here (so we don't loop infinitely
      }
    }

    if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
      // simple pipe
      int n = readFifo(INBOARD_RX);

      if (n > 0) {
        if (!validRead())
          return;
        
        Command cmd(fifo_msg.data);

        if ( !cmd.isValid() ) {
          cout << "nidaq: invalid cmd" << endl;
        } else {
          switch( cmd.cmd() ) {
            case CMD_NIDAQ_ACTION: {
              printf("nidaq: recieved CMD_NIDAQ_ACTION\n");
              char channel[100];

              NIDAQAction_t *action = (NIDAQAction_t*)CMD_DATAPTR(fifo_msg.data);
              sprintf(channel, "Dev0/port%d/line%d", action->port, action->line);
              printf("writting to channel \'%s\'\n", channel);

              //writeFifo(OUTBOARD_TX);
              if (num_connections > 0) {
                printf("sending magic\n");
                send(client_fds[0], (void*)action, sizeof(NIDAQAction_t), 0);
              }

              //digitalWrite(channel, (char*)action->buf, action->len);
            }
            default: {
              printf("nidaq: INBOARD_RX got unknown (%X) on INBOARD_RX\n", cmd.cmd());
            }
          }
        }
      }
    }
  }
}

void NIDAQ::hexDump(char *buf, int len) {
  for (int i =0; i < len; i++)
    printf("%X ", buf[i]);
  printf("\n");
}

bool NIDAQ::validRead() {
  uint16_t length = CMD_SIZE(fifo_msg.data);
  if( length != fifo_msg.size) {
    cout << getName() << "::update() - error, length of command != length of buffer" << endl
    << "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

    if( length < 1000 && length <= fifo_msg.size ){
      if( C_calcCheckSum((uint8_t *)buf) == 0 )
        cout << "\tCMD: is valid" << endl;
      else
        cout << "\tCMD: is NOT valid" << endl;
    }

    return false;
  }

  return true;
}


void* NIDAQ::acceptConnections(void *data) {
  struct sockaddr_in client;
  int len = sizeof(client);
  int r;

  NIDAQ *me = (NIDAQ*)data;
  
  while (1) {
    printf("waiting for connections (servSocket is %d)\n", me->servSocket);
    r = accept(me->servSocket, (struct sockaddr*)&client, (socklen_t*)&len);
    if (r < 0) {
      printf("accept error: %s\n", strerror(errno));
    } else {
      me->client_fds[me->num_connections] = r;
      me->num_connections = me->num_connections + 1;
      printf("accepted a connection (%d)\n", me->num_connections);
    }
  }
}
